#include "myFilter.h"

//-----------------------------------------------------------------------------------------------
int filtfilt(double* x, double* y, int xlen, double* a, double* b, int nfilt){
	double *tx,*tx1,*p,*t,*end;
	double *sp,*tvec,*zi;
	double tmp,tmp1;
	double *zi_temp;

	int nfact = nfilt-1;   //filter order
	int tlen;    //length of tx
	int i, j;

	if( xlen <= 3*nfact || nfilt < 2)  //the length of input sequence xlen should be larger than 3*nfact,filter order nfact = nfilt-1 should be larger than 1;   
		return -1;    

	//Extrapolate beginning and end of data sequence using a "reflection
	//method". Slopes of original and extrapolated sequences match at
	//the end points.
	//This reduces end effects.

	tlen = 6*nfact+xlen;

	tx = (double *)malloc(tlen*sizeof(double));
	tx1 = (double *)malloc(tlen*sizeof(double));

	sp = (double *)malloc( sizeof(double) * nfact * nfact );

	tvec = (double *)malloc( sizeof(double) * nfact );
	zi = (double *)malloc( sizeof(double) * nfact );
        zi_temp =  (double *)malloc( sizeof(double) * nfact );

	if( !tx || !tx1 || !sp || !tvec || !zi||!zi_temp )
	{
		free(tx);
		free(tx1);
		free(sp);
		free(tvec);
		free(zi);
		free(zi_temp);
		return 1;
	}



	// construct a new signal sequence tx,whose length is 6*nfact+xlen
	tmp=x[0];

	for(p = x+3*nfact,t= tx;p>x;--p,++t)
	{
		*t = 2.0*tmp - *p;
	}

	for(end=x+xlen;p<end;++p,++t)
	{	
		*t = *p;
	}

	tmp=x[xlen-1];

	for(end=tx+tlen,p-=2;t<end;--p,++t)
	{	
		*t = 2.0*tmp - *p;
	}
	//now tx is ok.



	end = sp + nfact*nfact;// "end" points to one past the last element of sp
	p=sp;

	while(p<end)
	{
		*p++ = 0.0L; // assign zero to each element of the matrix pointed by p 
	}



	// construct a matrix "sp" analogous to "E-P"
	// reference "零相移滤波器的改进及实现方法 陈淑珍 杨涛"
	/*sp[0]=1.0+a[1];
	for(i=1;i<nfact;i++)
	{
		sp[i*nfact]= a[i+1];
		sp[i*nfact+i] = 1.0;
		sp[(i-1)*nfact+i] = -1.0;
	}*/
	for(i=0; i<nfact; i++)
	{
		sp[i] = 0.5;
	}
	for(i=1;i<nfact;i++)
	{
		for(j=i; j<nfact; j++)
		{
			sp[i*nfact+j] = 1;
		}
	}
	
	// construct a vector "tvec" analogous to Q
	for(i=0;i<nfact;i++)
	{
		tvec[i]=b[i+1]-a[i+1]*b[0];
	}


	/*if(invMat(sp,nfact))//solve the inverse of "sp"
	{
		free(zi);
		zi=NULL;// if "sp" is not invertible, initial condition "zi" is null
	} */
	//else
	//{
        mulMat(sp,tvec,zi,nfact,nfact,1);//initial condition "zi" is solved by the inverse of "sp" and "tvec"
	//}

	free(sp);
	free(tvec);

  	//filtering tx, save it in tx1
  	tmp1 = tx[0];

	if(zi)
	{
		for(i=0; i<nfact; i++)
		{
			zi_temp[i] =  zi[i]*tmp1;
			//*(p++) *= tmp1;//?
		}
	}

	filter(tx,tx1,tlen,a,b,nfilt,zi_temp);

	//reverse tx1
	for( p=tx1,end=tx1+tlen-1; p<end; p++,end--)
	{
		tmp = *p;
		*p = *end;
		*end = tmp;
	}

	//filter again
	tmp1 = tx1[0];
        //tmp1 = (*tx1)/tmp1;
	if(zi)
	{
 //		for( p=zi,end=zi+nfact; p<end;)
		for(i=0; i<nfact; i++)
		{
			zi_temp[i] =  zi[i]*tmp1;
			//*(p++) *= tmp1;//?
		}
	}

	filter(tx1,tx,tlen,a,b,nfilt,zi_temp);

	//reverse to y
	end = y+xlen;
	p = tx+3*nfact+xlen-1;
	while(y<end)
	{
		*y++ = *p--;
	}

	free(zi);
	free(zi_temp);
	free(tx);
	free(tx1);

	return 0;
}
//-----------------------------------------------------------------------------------------------
void filter(double* x, double* y, int xlen, double* a, double* b, int nfilt, double* zi)
{
	double tmp;
	int i,j;

	double* x1 = new double[xlen];
	memcpy(x1, x, sizeof(double)*xlen);

	//normalization
	if( (*a-1.0>EPS) || (*a-1.0<-EPS) )
	{
		tmp=*a;
		for(i=0;i<nfilt;i++)
		{
			b[i]/=tmp;
			a[i]/=tmp;
		}
	}



	memset(y,0,xlen*sizeof(double));//initialize the array pointed by "y" with 0


	// solve output sequence using time domain difference equations
	// seeing the Matlab help document about the command "filter"
	a[0]=0.0;

	for(i=0;i<xlen;i++)
	{
		for(j=0;i>=j&&j<nfilt;j++)
		{
			y[i] += (b[j]*x1[i-j]-a[j]*y[i-j]);
		}

		if(zi&&i<nfilt-1)
			y[i] += zi[i];
	}

	a[0]=1.0;

	delete[] x1; 
	return;

}

//-----------------------------------------------------------------------------------------------
int invMat(double *a, int n)
{ 
	int *is,*js,i,j,k,l,u,v;
	double d,p;

	is = (int *)malloc(n*sizeof(int));
	js = (int *)malloc(n*sizeof(int));

	for (k=0; k <= n-1; k++)
	{
		d=0.0;
		for (i=k; i<=n-1; i++)
		{
			for (j=k; j<=n-1; j++)
			{ 
				l=i*n+j; 
				p=fabs(a[l]);
				if (p>d) 
				{ 
					d=p; 
				    is[k]=i;
					js[k]=j;
				}
		    }
		}

		if (d+1.0==1.0)
		{ 
			free(is); 
			free(js); 
			// printf("err**not inv\n");
			return 1;// matrix is not invertible!
		}

		if (is[k]!=k)
		{
			for (j=0; j<=n-1; j++)
			{
				u=k*n+j;
				v=is[k]*n+j;
				p=a[u];
				a[u]=a[v]; 
				a[v]=p;
			}
		}

		if (js[k]!=k)
		{
			for (i=0; i<=n-1; i++)
			{ 
				u=i*n+k;
				v=i*n+js[k];
				p=a[u];
				a[u]=a[v]; 
				a[v]=p;
			}
		}


		l=k*n+k;
		a[l]=1.0/a[l];

		for (j=0; j<=n-1; j++)
		{
			if (j!=k)
			{ 
				u=k*n+j;
				a[u]=a[u]*a[l];
			}
		}
		
		for (i=0; i<=n-1; i++)
		{
			if (i!=k)
			{
				for (j=0; j<=n-1; j++)
				{
					if (j!=k)
					{ 
						u=i*n+j;
						a[u] = a[u]-a[i*n+k]*a[k*n+j];
					}
				}
			}
		}

		for (i=0; i<=n-1; i++)
		{
			if (i!=k)
			{ 
				u=i*n+k; 
				a[u]=-a[u]*a[l];
			}
	    }

	}

	for (k=n-1; k>=0; k--)
	{
		if (js[k]!=k)
		{
			for (j=0; j<=n-1; j++)
			{ 
				u=k*n+j; 
				v=js[k]*n+j;
				p=a[u];
				a[u]=a[v];
				a[v]=p;
			}
		} 

		if (is[k]!=k)
		{
			for (i=0; i<=n-1; i++)
			{ 
				u=i*n+k; 
				v=i*n+is[k];
				p=a[u];
				a[u]=a[v]; 
				a[v]=p;
			}
		}
	}
	
  free(is); 
  free(js);
  return 0;// matrix is invertible!
}
//-----------------------------------------------------------------------------------------------
void mulMat(double *a,double *b,double *c,int m,int n,int k)
{
	int i,j,t,u;
	for (i=0;i<=m-1;i++)
	{
		for (j=0;j<=k-1;j++)
		{
			u = i*k+j;
			c[u] = 0.0;
			for (t=0;t<= n-1;t++)
			{
				c[u] = c[u] + a[i*n+t]*b[t*k+j];
			}
		}
	}

	return;

}
//-----------------------------------------------------------------------------------------------
